/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|									Copyright (C) 2009 Jack Elston, Cory Dixon                   |
|                                                                              |
      This program is free software; you can redistribute it and/or             
      modify it under the terms of the GNU General Public License               
      as published by the Free Software Foundation; either version 2            
      of the License, or (at your option) any later version.                    
                                                                                
      This program is distributed in the hope that it will be useful,           
      but WITHOUT ANY WARRANTY; without even the implied warranty of            
      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             
      GNU General Public License for more details.                              
                                                                                
      You should have received a copy of the GNU General Public License         
      along with this program; if not, write to the Free Software               
      Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA                 
      02111-1307, USA.                                                          
                                                                                
 			  		Jack Elston                       Cory Dixon                        
|			  		elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * Thread.cxx
 *
 * PURPOSE:
 *   Implemnts a thread class object using pThreads.
 *
 * CREATED:
 * 11/2000 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/11/02 20:27:20 $
 * $Revision: 1.8 $
 *
 ***********************************************************************/

// standard headers
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <signal.h>

#include <iostream>

// include project headers
#include "Thread.h"
#include "color.h"

#define USE_TSIGQUIT_SIGHANDLER

#ifdef IS_TARGET
 #undef USE_TSIGQUIT_SIGHANDLER
#endif


//----------------------------------------------------------
// initialize static member variables
//----------------------------------------------------------
unsigned int pThread::threadNumber = 0;

//----------------------------------------------------------
// Constructor
//----------------------------------------------------------
pThread::pThread(String nameStr) 
{
	name		= nameStr;

	tid      = 0; 	
	pid		= 0; 	
	execRate	= -1.0; 		// desired run rate
	runRate  = 0.0; 		// actual run rate
	runTime  = 0.001; 	// no division by zero
	runCount	= 0; 
	shmPtr   = NULL;
	running  = false;		// we are not running till later
	itPtr    = NULL;

	threadNumber++;
	myNumber = threadNumber;

#ifdef DEBUG_PRINT
	cout << "pThread::pThread - " << myNumber << " name=" << name 
		<< " (pid=" << getpid() << " tid=" << pthread_self() << ")" 
		<< endl;
#endif
}

//----------------------------------------------------------
// Destructor  
//----------------------------------------------------------
pThread::~pThread()
{ 	
	//cout << "pThread::~pThread()" << endl;

	tid=0;
	pid=0;
	mutex_unlock();
}

//----------------------------------------------------------
// initialize thread
//  call before start
//----------------------------------------------------------
bool pThread::init( ShmStruct * shmStructPtr )
{ 
	// set up the global area 
	shmPtr = shmStructPtr;
	if( shmPtr ) {
		p_mtx = shmPtr->mtx; 
	} else {
		pthread_mutex_init(&p_mtx, NULL);
	}

	return true;
}

//----------------------------------------------------------
// start the thread
//  only call this once
//----------------------------------------------------------
int pThread::start(float rate, int prio) 
{ 	
	int status=0;
	pthread_attr_t *attrPtr = NULL;

	// set the desired execution
	execRate = rate;

	// Determine the running priority of the thread
	if (prio < 0 )
		return OK;
	else if( prio >= sched_get_priority_min(SCHED_FIFO) && 
			prio <= sched_get_priority_max(SCHED_FIFO) ) 
	{
		if( pthread_attr_init( &attr ) != 0 )
			perror("pthread_attr_init");
		if( sched_getparam(0, &param) != 0 )
			perror("sched_getparam");

		param.sched_priority = prio;

		// realtime, first-in first-out
		if( pthread_attr_setschedpolicy( &attr, SCHED_FIFO) != 0 )
			perror("pthread_attr_setschedpolicy");
		if( pthread_attr_setschedparam( &attr, &param) != 0 )
			perror("pthread_attr_setschedparam");

		// set pointer
		attrPtr = &attr;
	} else if(prio != 0)
		return ERROR;

#ifdef DEBUG_PRINT
	cout << "Spawning " << name << " (rate=" << rate << " prio=" << prio << ")" << endl << flush;
#endif

	status = pthread_create(&tid, attrPtr, startFunc, (void *)this); 

	if( status != 0) {
		if( status == EPERM )
			cout << endl << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl
				<< "!! You must have su permisions !!" << endl
				<< "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl << flush;
		else
			perror("pthread_create");
		return ERROR;
	}

	return OK;
}

//----------------------------------------------------------
// member variable get functions
//----------------------------------------------------------
pthread_t pThread::getTid() const
{ 	
	return tid; 		
}

pid_t pThread::getPid() const
{ 	
	return pid; 		
}

String pThread::getName() const
{ 	
	return name; 		
}

//----------------------------------------------------------
// shut down nicely
//----------------------------------------------------------
void pThread::shutdown(void)
{ 	
#ifdef DEBUG_PRINT
	printf("pThread::shutdown - name=%s\n", name.c_str());
#endif

	// turn me off if on
	if( execRate >= 0 ) {
		//execRate = -1;

		// send signal to intterupt thread
		// if thread exsists, otherwise send it to the
		// main process
		if( tid > 0 ) {
			cout << "Stopping " RED << name << ATTROFF " ... " << endl;
			//cout << "Sending " << name << " (pid=" << pid << " tid=" << tid << ") " 
				//<< " shutdown signal" << endl;

#ifdef USE_TSIGQUIT_SIGHANDLER
#if(_POSIX_C_SOURCE >= 199309L)
			union sigval sval;
			sval.sival_ptr = this;

			if( sigqueue(pid, TSIGQUIT, sval) < 0)
				perror("pThread::shutdown sigqueue");
#else
			printf("pThread::shutdown invalid signalling\n");
#endif
#else
			if( pthread_kill( tid, TSIGQUIT ) < 0 )
				perror("pThread::shutdown sigqueue");
#endif

		} else {
#ifdef DEBUG_PRINT
			cout << "pThread::shutdown -- " << name << " not running" << endl;
#endif
			// exit thread
			this->exit();
		}
	}
}

void pThread::displayStats()
{
	setRunStat();

	cout << BLUE "\n<------------------------------ " YELLOW << name << BLUE " ------------------------------>\n" ATTROFF << flush;
	//
	// Display some stats
	//
	cout << "Thread Stats:" << endl << "--------------------" << endl
		<< "\tTime = " CYAN << runTime <<  ATTROFF " [s]" 
		<< "\t\tCount = " CYAN << runCount <<  ATTROFF " [#]" << endl
		<< "\tRate = "  CYAN<< runCount / runTime <<  ATTROFF " [Hz]"
		<< "\t\t   dT = "  CYAN<<  timeStep / 1000.0 << ATTROFF " [ms]" << endl
		<< endl << flush;

	//cout << "pid=" << pid << " tid=" << tid << endl;
	cout << BLUE "================================================================"  << flush;
	for( unsigned int i=0; i < name.length(); i++)
		cout << "=";
	cout << ATTROFF "\n\n" << flush;
}


//----------------------------------------------------------
// get memory area
//----------------------------------------------------------
void pThread::getShm()
{	
	mutex_lock();

	// straight memory copy
	//shmVar = *shmPtr;

	mutex_unlock();
}

//----------------------------------------------------------
// set memory area, only what you need
//----------------------------------------------------------
void pThread::setShm()
{
	mutex_lock();

	// do something

	mutex_unlock();
}

void pThread::setRunning(bool run)
{
	if( itPtr == NULL ){
		if( execRate > 0) cout << "pThread::setRunning -- No time ptr" << endl;
		return;
	}

	// start timer
	if( run == true ) {
		if( itPtr->getRunRate() > 0 )
			itPtr->start();
	} else if( run == false ) { 
		itPtr->stop();
	}

	running = run;
}

//----------------------------------------------------------
// Infinte run loop 
//----------------------------------------------------------
void pThread::run(void)
{ 	

	// setup signals
	sigset_t waitSigs, pendingSigs;
	siginfo_t extra;
	int sig = -1;

#ifdef _POSIX_TIMERS
	struct sigevent se = {{0},0,0};
	se.sigev_signo = SIGRTMIN + myNumber;
	se.sigev_value.sival_ptr = this;
	se.sigev_notify = SIGEV_SIGNAL;
	if( execRate > 0 ) {
		itPtr = new IntervalTimer (execRate, &se);
	}
#else
	if( execRate > 0 ) 
		itPtr = new IntervalTimer (execRate, NULL);
#endif

#ifdef DEBUG_PRINT
	cout << name << ":"
		<< " Thread=" << myNumber 
		<< " runRate=" << execRate << " [Hz]"
		<< " pid=" << getpid() << " tid=" << pthread_self() 
		<< endl << flush;
#endif

	//
	// setup signals for the process
	//  TSIGTIMER - timer signal for running, better to use
	//  TSIGQUIT - from code to exit threads
	//  SIGINT - don't want int

	// block all signals
	sigfillset(&waitSigs);
	pthread_sigmask(SIG_BLOCK,&waitSigs,NULL);

#ifdef USE_TSIGQUIT_SIGHANDLER
	// except for TSIGQUIT which is used by the main function/thread
	sigemptyset(&waitSigs);
	sigaddset(&waitSigs,TSIGQUIT);
	pthread_sigmask(SIG_UNBLOCK,&waitSigs,NULL);
#endif

#ifdef DEBUG_PRINT
	cout << getName() << " signal=" << SIGRTMIN + myNumber << " for my number=" << myNumber << endl;
#endif
	sigemptyset(&waitSigs);
	sigaddset(&waitSigs,SIGRTMIN + myNumber);
	sigaddset(&waitSigs,TSIGTIMER);
#ifndef USE_TSIGQUIT_SIGHANDLER
	sigaddset(&waitSigs,TSIGQUIT);
#endif

	// this is what we use to interrupt anything, so
	// setup a real signal and a signal handler
#ifdef USE_TSIGQUIT_SIGHANDLER
	struct sigaction sa;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = SA_SIGINFO;
	sa.sa_sigaction = sigHandler;
	if( sigaction(TSIGQUIT, &sa, NULL) < 0)
		perror("pThread::run sigaction");
#endif

	//
	// start timer
	//
	setRunning(true);

	// make first time stamp
	lastStamp.stamp();
	currentStamp.stamp();
	startStamp.stamp();

	while(execRate >= 0)
	{
		//
		// wait for run signal
		//
		if( (execRate > 0) ) {
#ifdef DEBUG_PRINT
			cout << name << ": sigwaitinfo" << endl;
#endif
			if( (sig = sigwaitinfo(&waitSigs,&extra)) < 0) {
				if( (errno != EAGAIN) && (errno != EINTR) ) {
					cout << name; perror("pThread::run sigwaitinfo()");
					if( itPtr != NULL ) itPtr->stop();	
					execRate = -1;
				} else if( errno == EINTR ) {
#ifdef DEBUG_PRINT
					cout << name << ": sigwaitinfo interrupted" << endl;
#endif
				}
			}
		} else if( execRate == 0 ) {
			if(sigpending(&pendingSigs) < 0 ) perror("pThread::run sigpending()");

			if(sigismember(&pendingSigs, TSIGQUIT)) sig = TSIGQUIT;
			else sig = 0;
		}

#ifdef DEBUG_PRINT
		cout << name << ": sigwaitinfo - sig=" << sig  << " execRate=" << execRate 
			<< " TSIGTIMER=" << TSIGTIMER << endl;
#endif

		if ( ((sig == TSIGTIMER || sig == (int)(SIGRTMIN + myNumber)) && execRate > 0) 
				|| (execRate==0 && sig != TSIGQUIT) ){
			//
			// get the most up-to-date values
			//
			//this->getShm(); 

			//
			// process them
			//
			this->update();

			//
			// update run count/time => rate
			//
			setRunStat();

			//
			// provide output
			//
			//this->setShm();

		} else if(sig == TSIGQUIT) {
#ifdef DEBUG_PRINT
			cout << name << ": mainloop received exit signal=" << sig << endl;
#endif
			execRate = -1;
		} else if( sig >= 0) {
			// caught another signal, don't do anything
#ifdef DEBUG_PRINT
			cout << "\t" << name <<": mainloop received signal=" << sig << " " << endl;
#endif
		} 
	}

	// exit thread
	setRunning(false);
	this->exit();
}

//----------------------------------------------------------
// run statistics
//----------------------------------------------------------
void pThread::setRunStat()
{
	currentStamp.stamp();
	timeStep = currentStamp - lastStamp;
	runTime = (currentStamp - startStamp) / (float)SEC2MICRO;

	if( timeStep > 1e3 )
		runRate = 1 / (timeStep / (float)SEC2MICRO);

	runCount++;

	lastStamp = currentStamp;
}

//----------------------------------------------------------
// close any open interfaces
//----------------------------------------------------------
void pThread::close() 
{
	// Nothing to do for generic thread
}

//----------------------------------------------------------
// exit from the thread
//----------------------------------------------------------
void pThread::exit() 
{ 	
	// make sure run loop is shutdown
	execRate = -1;
	running = false;

	if( itPtr != NULL ) {
		itPtr->stop();
		delete itPtr;
		itPtr = NULL;
	}

	// close any open interfaces
	this->close();

	//
	// Display some stats
	//
	displayStats();

	// only exit if there is a thread
	if( tid > 0 ) {
		pthread_exit(NULL);	
	}
}

//----------------------------------------------------------
// this function gets called by pthread_create
// as a friend function.
//----------------------------------------------------------
void *startFunc( void *task )
{ 	
	// pointer passed to myself
	pThread *thread = (pThread *)task;

	thread->pid = getpid();
	thread->tid = pthread_self();

#ifdef DEBUG_PRINT
	cout << "\tTask IDs:  PID=" << (int) thread->pid << " TID=" <<  thread->tid << endl;
#endif

	// go into run loop
	thread->run();		

	cout << "###### should never be here ###### " << endl;
	// unless the main loop exited, and we simply closed
	return NULL;
}

//----------------------------------------------------------
// catch signals
//----------------------------------------------------------
void sigHandler(int signo, siginfo_t *info, void *extra)
{
#ifdef DEBUG_PRINT
	cout << "sigHandler: pid=" << getpid() << " tid=" << pthread_self() << endl;
#endif

	pThread *me = (pThread *) info->si_value.sival_ptr;

	if( me != NULL ) {
#ifdef DEBUG_PRINT
		cout << me->getName() << ": sigHandler caught signal " << signo << endl << flush;
#endif
		if( signo == TSIGQUIT ) {
			me->execRate = -1;
		}

	} 
#ifdef DEBUG_PRINT
	else
		cout << "ERROR: no valid thread pointer for signal " << signo << endl << flush;
#endif

}


//----------------------------------------------------------
// Update values
//----------------------------------------------------------
void pThread::update()
{
	// this function does nothing
	float val=9999999;
	for(int i=0; i < 1000; i++)
		val *= 0.99;
}  

//----------------------------------------------------------
// mutex locking functions
//----------------------------------------------------------
void pThread::mutex_lock()
{
	pthread_mutex_lock(&p_mtx);
}

void pThread::mutex_unlock()
{
	pthread_mutex_unlock(&p_mtx);
}

